#include <Servo.h>

#define MAX_COUNT 8
Servo servos[MAX_COUNT];
byte servosCount;
byte pinToServo[32];

void initAll() {
  servosCount = 0;
  for (byte i = 0; i < sizeof(pinToServo); i++)
    pinToServo[i] = MAX_COUNT;
}

void setup() {
  pinMode(13, OUTPUT);
  Serial.begin(115200);
  initAll();
}

int setInterval(byte pin, int interval) {
  if (pinToServo[pin] < servosCount)
    servos[pinToServo[pin]].writeMicroseconds(interval);
  else if (servosCount < MAX_COUNT) {
    servosCount++;
    servos[servosCount - 1].attach(pin);
    pinToServo[pin] = servosCount - 1;
    servos[servosCount - 1].writeMicroseconds(interval);
  } else
    return 0;
  return 1;
}

void resetAll() {
  for (byte i = 0; i < servosCount; i++)
    servos[i].detach();
  initAll();
}

int detachServo(byte pin) {
  if (pinToServo[pin] < servosCount) {
    byte idx = pinToServo[pin];
    servos[idx].detach();
    if (idx + 1 < servosCount) {
      int msec = servos[servosCount - 1].readMicroseconds();
      byte ppin = 0;
      for (;pinToServo[ppin] != servosCount - 1;ppin++) ;
      servos[servosCount - 1].detach();
      servos[idx].attach(ppin);
      pinToServo[ppin] = idx;
      servos[idx].writeMicroseconds(msec);
    }
    servosCount--;
    pinToServo[pin] = MAX_COUNT;
  } else
    return 0;
  return 1;
}

char prev_c = 0;
int readNumber() {
  char c;
  int interval = 0;
  int sign = 1;
  while (1) {
    while ((c = Serial.read()) < 0) ;
    prev_c = c;
    if (c >= '0' && c <= '9')
      interval = interval * 10 + c - '0';
    else if (c == '-')
      sign = -sign;
    else if (c == ' ' || c == 10)
      break;
  }
  return sign * interval;
}

int processCommand() {
  char c;
  char buf[20] = {0};
  byte len = 0;
  int res = 0;

  while (1) {
    while ((c = Serial.read()) < 0) ;
    prev_c = c;
    if (c == ' ' || c == 10)
      break;
    if (c >= 'A' && c <= 'Z' || c >= 'a' && c <= 'z' || c == '-' || c >= '0' && c <= '9')
      buf[len++] = c;
  }
  buf[len] = 0;

// handle commands
  if (!strcmp(buf, "servo") || !strcmp(buf, "SERVO")) {
    byte pin = readNumber();
    res = setInterval(pin, readNumber());
  } else if (!strcmp(buf, "analog") || !strcmp(buf, "ANALOG")) {
    res = analogRead(readNumber());
  } else if (!strcmp(buf, "reset") || !strcmp(buf, "RESET")) {
    resetAll();
    res = 1;
  } else if (!strcmp(buf, "detach") || !strcmp(buf, "DETACH")) {
    byte pin = readNumber();
    res = detachServo(pin);
  }

// feed
  while (1) {
    while ((c = Serial.read()) < 0) ;
    if (c == 10 && prev_c == 10)
      break;
    prev_c = c;
  }
  return res;
}

void loop() {
  int res = processCommand();
  digitalWrite(13, (res ? LOW : HIGH));
  Serial.print("OK ");
  Serial.print(res);
  Serial.println();
  Serial.println();
}
